package robot;


import java.awt.Color;
import java.util.ArrayList;
import java.util.Random;

import renderables.RenderableOval;
import renderables.RenderablePoint;
import renderables.RenderablePolygon;
import renderables.RenderablePolyline;
import dataStructures.RRNode;
import dataStructures.RRTree;
import easyGui.EasyGui;
import geometry.IntPoint;

public class RRT {
	private final EasyGui gui;
	private RRTree tree;
	
	private IntPoint goldPoint; 
	private IntPoint startPoint;	
	
	public IntPoint[] raysPoint = new IntPoint[7];
	public int[] distancePoint = new int[7];
	private int goalRadius;
	
	private IntPoint middlePointForField;
	private int radiusForField;
	
	Obstacle obstacle1;
	Obstacle obstacle2;
	Obstacle obstacle3;
	Obstacle obstacle4;
	Obstacle obstacle5;
	Obstacle obstacle6;
	Obstacle obstacle7;
	private ArrayList<Obstacle> obstacles = new ArrayList<Obstacle> ();
	private ArrayList<IntPoint> pointsForPath = new ArrayList<IntPoint> ();
	
	int textFieldX;
	int textFieldY;
	
	int numberOfNodes = 0;
	int numberOfExecutedNodes = 0;
	
	private ArrayList<RenderablePoint> randomPoints = new ArrayList<RenderablePoint> ();
	
	public RRT(EasyGui gui) {
		this.gui = gui;
		
		gui.clearGraphicsPanel();
		
		// Create an RRTree to be used later.
		tree = new RRTree(Color.BLACK);
	}
	

	public void runDemo() {
		// Displays the GUI i.e. makes it visible.
		gui.show();
	}
	
	
	public void checkXYParameters() {
		
		boolean  notFoundGoodPoints = true;
		
		while(notFoundGoodPoints) {
			Random generator = new Random();
			textFieldX = generator.nextInt(800);
			textFieldY = generator.nextInt(800);
			if (!checkIfInObstacle (new IntPoint(textFieldX, textFieldY))) {
				notFoundGoodPoints = false;
			}
		}
	}
	
	public void freeSpaceTree() {

		checkXYParameters();
		numberOfNodes = 0;
		numberOfExecutedNodes = 0;
		
		System.out.println("You choose free space RRT");
		
		gui.clearGraphicsPanel();
		
		// Create an RRTree to be used later.
		tree = new RRTree(Color.BLACK);

		gui.draw(tree);

		startPoint = new IntPoint(textFieldX, textFieldY);
		goldPoint = new IntPoint(400, 400);
		goalRadius = 50;
		specifyField();
		
		// set start and gold points 
		tree.setStartAndGoal(startPoint, goldPoint, goalRadius);
		
		makeFreeSpaceRRT();
		
		gui.update();
		System.out.println("Number of nodes: " + numberOfNodes);
		System.out.println("Number of executed nodes: " + numberOfExecutedNodes);
		
		double ratio = 1;
		if (numberOfExecutedNodes != 0) {
		 ratio = numberOfNodes / numberOfExecutedNodes;
		}
		System.out.println("Ratio: " + ratio);
	}
	
	public void obstacleSpace() {
		
		checkXYParameters();
		numberOfNodes = 0;
		numberOfExecutedNodes = 0;
		
		System.out.println("You choose RRT with obstacles");
		
		gui.clearGraphicsPanel();

		tree = new RRTree(Color.BLACK);

		gui.draw(tree);
		
		startPoint = new IntPoint(textFieldX, textFieldY);
		goldPoint = new IntPoint(400, 400);
		goalRadius = 50;
		specifyField();
		
		// set start and gold points 
		tree.setStartAndGoal(startPoint, goldPoint, goalRadius);
		
		
		Obstacle obstacle1 = new Obstacle(80, 200, 300, 200, 300, 120, 80, 120);
		obstacles.add(obstacle1);
		RenderablePolygon obstaclePolygon1 = obstacle1.print();
		gui.draw(obstaclePolygon1);
		
		 obstacle2 = new Obstacle(400, 400, 460, 400, 460, 440, 400, 440);
		obstacles.add(obstacle2);
		RenderablePolygon obstaclePolygon2 = obstacle2.print();
		gui.draw(obstaclePolygon2);

		
		Obstacle obstacle3 = new Obstacle(400, 300, 460, 300, 460, 330, 400, 330);
		obstacles.add(obstacle3);
		RenderablePolygon obstaclePolygon3 = obstacle3.print();
		gui.draw(obstaclePolygon3);
		
//		obstacle4 = new Obstacle(240, 240, 300, 240, 300, 280, 240, 280);
//		obstacles.add(obstacle4);
//		RenderablePolygon obstaclePolygon4 = obstacle4.print();
//		gui.draw(obstaclePolygon4);
		
		Obstacle obstacle5 = new Obstacle(300, 480, 330, 480, 330, 410, 300, 410);
		obstacles.add(obstacle5);
		RenderablePolygon obstaclePolygon5 = obstacle5.print();
		gui.draw(obstaclePolygon5);
		
		Obstacle obstacle6 = new Obstacle(550, 550, 600, 550, 600, 600, 550, 600);
		obstacles.add(obstacle6);
		RenderablePolygon obstaclePolygon6 = obstacle6.print();
		gui.draw(obstaclePolygon6);
		
		Obstacle obstacle7 = new Obstacle(580, 300, 630, 300, 630, 380, 580, 380);
		obstacles.add(obstacle7);
		RenderablePolygon obstaclePolygon7 = obstacle7.print();
		gui.draw(obstaclePolygon7);
		
		Obstacle obstacle8 = new Obstacle(100, 650, 160, 650, 160, 700, 100, 700);
		obstacles.add(obstacle8);
		RenderablePolygon obstaclePolygon8 = obstacle8.print();
		gui.draw(obstaclePolygon8);
		
		makeObstacleRRT();
		gui.update();
		
		System.out.println("Number of nodes: " + numberOfNodes);
		System.out.println("Number of executed nodes: " + numberOfExecutedNodes);
		
		double ratio = 1;
		if (numberOfExecutedNodes != 0) {
		 ratio = numberOfNodes / numberOfExecutedNodes;
		}
		System.out.println("Ratio: " + ratio);
	}
	
	public void treeExample() {
		// Set the start position to (100, 100) and the goal position to (300, 300).
		// The goal radius is 50. A path that ends in the circle of this radius around
		// the goal is considered to have attained the goal position.
		startPoint = new IntPoint(0, 0);
		goldPoint = new IntPoint(300, 300);
		goalRadius = 50;
		specifyField();
		
		// set start and gold points 
		tree.setStartAndGoal(startPoint, goldPoint, goalRadius);
		
		makeObstacleRRT();
		
		gui.update();
	}
	
	public void makeFreeSpaceRRT() {

		// for first step: nearest is start point
		RRNode nearest = tree.getNearestNeighbour(startPoint);
		tree.addNode(nearest, startPoint);
		numberOfNodes ++;
		
		// find point
		IntPoint nearestPoint = makeStep();

		// add this point to start point
		tree.addNode(nearest, nearestPoint);
		numberOfNodes ++;
		
		boolean NotReachedGold = false;
		IntPoint nearestPoint1 = null;
		while (!NotReachedGold) {
			nearestPoint1 = makeStep();
			NotReachedGold = checkIfReachGoldCircle(nearestPoint1);
			numberOfNodes ++;
		}
		

		RRNode lastNode = tree.getNearestNeighbour(nearestPoint1);
		// all point from start to End
		pointsForPath = tree.getPathFromRootTo(lastNode);

		RenderablePolyline executeLine = new RenderablePolyline();
		executeLine.setProperties(Color.BLUE, 8.0f);
		executeLine.addPoint(new RenderablePoint(startPoint.x, startPoint.y));
		for (int i = 1; i < pointsForPath.size(); i++) {
			IntPoint pointFromArray = pointsForPath.get(i);
			executeLine.addPoint(new RenderablePoint(pointFromArray.x, pointFromArray.y));		
		}
		
		numberOfExecutedNodes = pointsForPath.size();
		gui.draw(executeLine);

		gui.draw(tree);
		
	}


	public void makeObstacleRRT() {
		// make obstacles
		obstacle1 = new Obstacle(80, 200, 300, 200, 300, 120, 80, 120);
		obstacles.add(obstacle1);
		RenderablePolygon obstaclePolygon1 = obstacle1.print();
		gui.draw(obstaclePolygon1);
		
		// for first step: nearest is start point
		RRNode nearest = tree.getNearestNeighbour(startPoint);
		tree.addNode(nearest, startPoint);
		numberOfNodes ++;
		
		// find point
		IntPoint nearestPoint = makeObstacleStep();

		// check if this point intersect with obstacles 
		if (!checkIfInObstacle(nearestPoint)) {
			// if no intersections
			// add this point to start point
			tree.addNode(nearest, nearestPoint);
		}
	
		boolean NotReachedGold = false;
		IntPoint nearestPoint1 = null;
		while (!NotReachedGold) {
			nearestPoint1 = makeObstacleStep();
			numberOfNodes ++;
			NotReachedGold = checkIfReachGoldCircle(nearestPoint1);
		}
		
		RRNode lastNode = tree.getNearestNeighbour(nearestPoint1);
		// all point from start to End
		pointsForPath = tree.getPathFromRootTo(lastNode);

		RenderablePolyline executeLine = new RenderablePolyline();
		executeLine.setProperties(Color.BLUE, 8.0f);
		executeLine.addPoint(new RenderablePoint(startPoint.x, startPoint.y));
		for (int i = 1; i < pointsForPath.size(); i++) {
			IntPoint pointFromArray = pointsForPath.get(i);
			executeLine.addPoint(new RenderablePoint(pointFromArray.x, pointFromArray.y));		
		}
		numberOfExecutedNodes = pointsForPath.size();
		
		gui.draw(executeLine);
	}
	
	public boolean checkIfInObstacle(IntPoint pointToCheck) {

		int i;
		int j;
		boolean result = false;
		for (Obstacle obstacle : obstacles) {
			for (i = 0, j = obstacle.vertex.length - 1; i < obstacle.vertex.length; j = i++) {
				if ((obstacle.vertex[i].y > pointToCheck.y) != (obstacle.vertex[j].y > pointToCheck.y)
						&& (pointToCheck.x < (obstacle.vertex[j].x - obstacle.vertex[i].x)
								* (pointToCheck.y - obstacle.vertex[i].y)
								/ (obstacle.vertex[j].y - obstacle.vertex[i].y)
								+ obstacle.vertex[i].x)) {
					result = !result;
				}
			}
			if (result) {
				return result;
			}
		}
		System.out.println("result: " + result);
		return result;
	}
	
	// make a step towards the best potential distance
	public IntPoint makeObstacleStep () {
		// get random point
		IntPoint randomPoint = makeRandomPoint();
		System.out.println("Random point: " + randomPoint.x + "," + randomPoint.y);
		
		// get node from tree that is the most close to random point 
		RRNode n1 = tree.getNearestNeighbour(randomPoint);
		System.out.println("Nearest point: " + n1.x + "," + n1.y);
		
		int halfDistancePointX = (int) (randomPoint.x + 0.5 * (n1.x - randomPoint.x));
		int halfDistancePointY = (int) (randomPoint.y + 0.5 * (n1.y - randomPoint.y));

		
		IntPoint iP = new IntPoint( halfDistancePointX, halfDistancePointY);
		System.out.println("coordinates: " + halfDistancePointX + "," + halfDistancePointY);
		
		// if not intersects -> add
		if (!checkIfInObstacle(iP)) {
			
			boolean isIntersected = false;
			for (Obstacle obstacle: obstacles) {
				if (calculateHitsWithObstacle (new IntPoint(n1.x, n1.y), iP, obstacle)) {
					isIntersected = true;
				}
			}			
			// check for intersections
			if (!isIntersected) {
			
			// if no intersections
			// add this point to start point
			tree.addNode(n1, iP);
			System.out.println();
			}
		}
		return iP;
	}
	
	public IntPoint makeStep () {
		// get random point
		IntPoint randomPoint = makeRandomPoint();
		System.out.println("Random point: " + randomPoint.x + "," + randomPoint.y);
		
		// get node from tree that is the most close to random point 
		RRNode n1 = tree.getNearestNeighbour(randomPoint);
		System.out.println("Nearest point: " + n1.x + "," + n1.y);
		
		int halfDistancePointX = (int) (randomPoint.x + 0.5 * (n1.x - randomPoint.x));
		int halfDistancePointY = (int) (randomPoint.y + 0.5 * (n1.y - randomPoint.y));

		
		IntPoint iP = new IntPoint( halfDistancePointX, halfDistancePointY);
		System.out.println("coordinates: " + halfDistancePointX + "," + halfDistancePointY);

		tree.addNode(n1, iP);
		
		return iP;
	}
	
	public void specifyField() {
		
// 		middle point
		int middlePointX = (int) (startPoint.x + 0.5 *(goldPoint.x - startPoint.x));
		int middlePointY = (int) (startPoint.y + 0.5 *(goldPoint.y - startPoint.y));
		
//      Claculating distance between startPoint and middle 		
		int diffX  = middlePointX - startPoint.x;
		int diffY  = middlePointY - startPoint.y;
		
		double dist = Math.sqrt(Math.pow(diffX, 2) + Math.pow(diffY, 2));
		
		radiusForField =  (int) (dist * 3);
		middlePointForField = new IntPoint (middlePointX, middlePointY);
	}
	
	// fill the array with point of X-Ray
	public IntPoint makeRandomPoint() {	
		
		RenderableOval rendOv = new RenderableOval(middlePointForField.x, middlePointForField.y, radiusForField, radiusForField);
		gui.draw(rendOv);

		// need random angle between 0 - 360
		// need random number between 0 - radius
		
		Random randomGenerator = new Random();
		
		int r = randomGenerator.nextInt(radiusForField/2);
		int angle = randomGenerator.nextInt(360);
		
		System.out.println("r: " + r);
		System.out.println("angle " + angle);
		
		int Xpoint = (int) (middlePointForField.x + r * Math.cos(Math.toRadians(angle)));
		int Ypoint = (int) (middlePointForField.y + r * Math.sin(Math.toRadians(angle)));
		
		System.out.println("Random Point: " + Xpoint + " , " + Ypoint);
		
		RenderablePoint rp = new RenderablePoint(Xpoint, Ypoint);
		rp.setProperties(Color.RED, 10.0f);
		randomPoints.add(rp);
		
		gui.draw(rp);

		return new IntPoint(Xpoint, Ypoint);
	}
	
	// check for each IntPoint in X-Rays array if it is in gold Circle
	// if yes, initialise gold Point
	public boolean checkIfReachGoldCircle(IntPoint point) {
		if ((int) (Math.pow(point.x - goldPoint.x, 2) + Math.pow(
				point.y - goldPoint.y, 2)) <= Math.pow(goalRadius, 2)) {
			return true;
		} else {
			return false;
		}
	}
	
	// do this function for all XRAYS
	public boolean calculateHitsWithObstacle (IntPoint closestNodePoint, IntPoint futureStepPoint,  Obstacle obstacle) {
		
		double x1 = closestNodePoint.x;
		double y1 = closestNodePoint.y;
		double x2 = futureStepPoint.x;
		double y2 = futureStepPoint.y;
		
		for (int i=0; i<4; i++) {
			double x3 = obstacle.vertex[i].x;
			double y3 = obstacle.vertex[i].y;
			double x4 = obstacle.vertex[(i+1)%4].x;
            double y4 = obstacle.vertex[(i+1)%4].y;
			if (linesIntersect(x1,y1,x2,y2,x3,y3,x4,y4)) {
				return true;
			}
		}
		
		return false;
	}
	
	public static boolean linesIntersect(double x1, double y1, double x2, double y2, double x3, double y3, double x4, double y4){
	      // Return false if either of the lines have zero length
	      if (x1 == x2 && y1 == y2 ||
	            x3 == x4 && y3 == y4){
	         return false;
	      }
	      // Fastest method, based on Franklin Antonio's "Faster Line Segment Intersection" topic "in Graphics Gems III" book (http://www.graphicsgems.org/)
	      double ax = x2-x1;
	      double ay = y2-y1;
	      double bx = x3-x4;
	      double by = y3-y4;
	      double cx = x1-x3;
	      double cy = y1-y3;

	      double alphaNumerator = by*cx - bx*cy;
	      double commonDenominator = ay*bx - ax*by;
	      if (commonDenominator > 0){
	         if (alphaNumerator < 0 || alphaNumerator > commonDenominator){
	            return false;
	         }
	      }else if (commonDenominator < 0){
	         if (alphaNumerator > 0 || alphaNumerator < commonDenominator){
	            return false;
	         }
	      }
	      double betaNumerator = ax*cy - ay*cx;
	      if (commonDenominator > 0){
	         if (betaNumerator < 0 || betaNumerator > commonDenominator){
	            return false;
	         }
	      }else if (commonDenominator < 0){
	         if (betaNumerator > 0 || betaNumerator < commonDenominator){
	            return false;
	         }
	      }
	      if (commonDenominator == 0){
	         // This code wasn't in Franklin Antonio's method. It was added by Keith Woodward.
	         // The lines are parallel.
	         // Check if they're collinear.
	         double y3LessY1 = y3-y1;
	         double collinearityTestForP3 = x1*(y2-y3) + x2*(y3LessY1) + x3*(y1-y2);   // see http://mathworld.wolfram.com/Collinear.html
	         // If p3 is collinear with p1 and p2 then p4 will also be collinear, since p1-p2 is parallel with p3-p4
	         if (collinearityTestForP3 == 0){
	            // The lines are collinear. Now check if they overlap.
	            if (x1 >= x3 && x1 <= x4 || x1 <= x3 && x1 >= x4 ||
	                  x2 >= x3 && x2 <= x4 || x2 <= x3 && x2 >= x4 ||
	                  x3 >= x1 && x3 <= x2 || x3 <= x1 && x3 >= x2){
	               if (y1 >= y3 && y1 <= y4 || y1 <= y3 && y1 >= y4 ||
	                     y2 >= y3 && y2 <= y4 || y2 <= y3 && y2 >= y4 ||
	                     y3 >= y1 && y3 <= y2 || y3 <= y1 && y3 >= y2){
	                  return true;
	               }
	            }
	         }
	         return false;
	      }
	      return true;
	   }
}